#ifndef VELODYNE_RECEIVER_H_SEPT_14_2007_SVL5
#define VELODYNE_RECEIVER_H_SEPT_14_2007_SVL5

#include "..\Network\Udp_Connection.h"
#include "..\Utility\FastDelegate.h"
#include "..\Coords\v3f.h"
#include "..\Time\Car_Timestamp_Sync.h"
#include <vector>
using namespace std;

#include "VelodyneCommon.h"

#define VELODYNE_PORT 2368
#define VELODYNE_REMOTE_IP "192.168.3.255"

typedef FastDelegate2<VelodyneScan&, void*> VelodyneCallbackType;

// the great big velodyne table
const double velodyne_top_block_laser_correction_factors[32][5]={
//  RCFSIN           VCFSIN        RCFCOS      VCFCOS     HCF     
  {-0.08636,      -0.124608004, 0.996263996, 0.99220605, 4}, //laser number 1
	{-0.049106667,  -0.118712821, 0.99879354,  0.992928631, -4},
  {0.049106667,    0.005547006, 0.99879354,  0.999984615, 4},
  {0.08636,        0.011486092, 0.996263996, 0.999934033, -4},
  {-0.012104643,  -0.112815635, 0.999926736, 0.993615938, 4},
  {0.025148753,   -0.106916449, 0.99968372,  0.994268009, -4},
  {-0.025651246,  -0.148168656, 0.999670953, 0.988962107, 4},
  {0.011602023,   -0.142281511, 0.999932694, 0.989826233, -4},
  {0.062151658,   -0.101015263, 0.998066717, 0.994884876, 4},
  {0.099406514,   -0.09511208,  0.995046906, 0.99546657, -4},
  {0.048604612,   -0.136392351, 0.998818097, 0.990654898, 4},
  {0.085859212,   -0.130501182, 0.99630728,  0.991448154, -4},
  {-0.087111141,  -0.053733971, 0.996198599, 0.998555287, 4},
  {-0.049859725,  -0.047814845, 0.99875623,  0.998856216, -4},
  {-0.100656848,  -0.089206901, 0.994921202, 0.996013117, 4},
  {-0.063405816,  -0.083299727, 0.997987827, 0.996524538, -4},
  {-0.012858566,  -0.041893728, 0.999917325, 0.999122072, 4},
  {0.024395002,   -0.035970618, 0.999702398, 0.999352848, -4},
  {-0.026404972,  -0.07739056,  0.999651328, 0.997000853, 4},
  {0.010848088,   -0.071479401, 0.999941158, 0.997442076, -4},
  {0.061399116,   -0.030045516, 0.998113294, 0.999548532, 4},
  {0.098656238,   -0.02411842,  0.995121574, 0.999709109, -4},
  {0.047851507,   -0.065566249, 0.99885446,  0.997848218, 4},
  {0.08510799,    -0.059651106, 0.996371733, 0.998219287, -4},
  {-0.087862232,   0.017427184, 0.996132636, 0.999848135, 4},
  {-0.050612755,   0.023370284, 0.998718353, 0.999726878, -4},
  {-0.101406972,  -0.018189329, 0.994845026, 0.99983456, 4},
  {-0.064158263,  -0.012258243, 0.997939736, 0.999924865, -4},
  {-0.013612482,   0.029315394, 0.999907346, 0.999570211, 4},
  {0.023641237,    0.035262518, 0.999720507, 0.999378084, -4},
  {-0.027158684,  -0.00632516,  0.999631135, 0.999979996, 4},
  {0.010094148,   -0.000390077, 0.999949053, 0.999999924, -4}};

const double velodyne_bottom_block_laser_correction_factors[32][5]={
//  RCFSIN           VCFSIN        RCFCOS      VCFCOS     HCF     
  {-0.12954,      -0.386515978, 0.991574197, 0.922282711, 4},
  {-0.07366,      -0.378262058, 0.997283412, 0.925698556, -4},
  {0.07366,       -0.199606138, 0.997283412, 0.979876211, 4},
  {0.12954,       -0.190845208, 0.991574197, 0.981620144, -4},
  {-0.018031287,  -0.369984899, 0.999837423, 0.929037768, 4},
  {0.037848854,   -0.361684557, 0.999283475, 0.932300531, -4},
  {-0.038351144,  -0.419297955, 0.999264324, 0.907848679, 4},
  {0.017528712,   -0.411137663, 0.99984636,  0.911573268, -4},
  {0.093479558,   -0.353361082, 0.995621199, 0.93548701, 4},
  {0.149363003,   -0.345014517, 0.98878243,  0.938597349, -4},
  {0.073158701,   -0.40295385,  0.997320312, 0.915220298, 4},
  {0.129041564,   -0.394746598, 0.991639186, 0.918790032, -4},
  {-0.130287592,  -0.285944588, 0.991476244, 0.958246154, 4},
  {-0.074411913,  -0.277414227, 0.99722759,  0.960750408, -4},
  {-0.150605425,  -0.336644901, 0.988593954, 0.941631675, 4},
  {-0.094730618,  -0.328252267, 0.995502943, 0.944590096, -4},
  {-0.018785142,  -0.268860924, 0.999823544, 0.963179009, 4},
  {0.037095401,   -0.260284668, 0.999311729, 0.965531921, -4},
  {-0.03910456,   -0.319836642, 0.999235124, 0.947472703, 4},
  {0.01677484,    -0.311398048, 0.999859292, 0.950279567, -4},
  {0.092728851,   -0.251685444, 0.995691398, 0.967809091, 4},
  {0.148617436,   -0.243063222, 0.988894766, 0.970010448, -4},
  {0.072406719,   -0.302936501, 0.997375189, 0.953010743, 4},
  {0.128293849,   -0.294452013, 0.991736199, 0.955666266, -4},
  {-0.131035111,  -0.182060979, 0.991377728, 0.983287242, 4},
  {-0.075163784,  -0.173253382, 0.997171202, 0.984877285, -4},
  {-0.151350764,  -0.234417995, 0.988480119, 0.972135898, 4},
  {-0.095481183,  -0.22574971,  0.995431235, 0.974185336, -4},
  {-0.019538985,  -0.164422345, 0.999809096, 0.986390031, 4},
  {0.036341927,   -0.155567788, 0.999339414, 0.987825219, -4},
  {-0.039857955,  -0.217058336, 0.999205356, 0.976158634, 4},
  {0.01602096,    -0.208343829, 0.999871656, 0.978055647, -4}};

// specific to unit#29...
// vertical corr (deg); rot corr(deg); distance corr(cm); vertical offset(cm); horiz offset(cm_
const double velodyne_unit29_corrections[5*64] ={
  -7.1581200000,-4.5000000000,26.0,0.0000000000,4,     // lower block
  -6.8178200000,-2.8000000000,40.0,0.0000000000,-4,
  0.3178220000,3.4000000000,32.0,0.0000000000,4,
  0.6581190000,4.9542300000,20.0,0.0000000000,-4,
  -6.4776500000,-0.1000000000,12.0,0.0000000000,4,
  -6.1375900000,1.6500000000,22.0,0.0000000000,-4,
  -8.5208100000,-1.0000000000,36.0,0.0000000000,4,
  -8.1798900000,0.5000000000,25.0,0.0000000000,-4,
  -5.7976400000,4.2000000000,23.0,0.0000000000,4,
  -5.4577700000,5.9000000000,25.0,0.0000000000,-4,
  -7.8391400000,3.2000000000,26.0,0.0000000000,4,
  -7.4985600000,5.4000000000,20.0,0.0000000000,-4,
  -3.0802100000,-4.5200000000,18.0,0.0000000000,4,
  -2.7406300000,-2.7000000000,29.0,0.0000000000,-4,
  -5.1179800000,-5.2000000000,20.0,0.0000000000,4,
  -4.7782600000,-3.6000000000,27.0,0.0000000000,-4,
  -2.4010400000,-0.2500000000,11.0,0.0000000000,4,
  -2.0614100000,1.7500000000,18.0,0.0000000000,-4,
  -4.4385900000,-1.0000000000,24.0,0.0000000000,4,
  -4.0989600000,0.9000000000,25.0,0.0000000000,-4,
  -1.7217400000,4.0000000000,17.0,0.0000000000,4,
  -1.3820200000,5.8500000000,19.0,0.0000000000,-4,
  -3.7593700000,3.2000000000,22.0,0.0000000000,4,
  -3.4197900000,5.1000000000,20.0,0.0000000000,-4,
  0.9985550000,-4.5000000000,25.0,0.0000000000,4,
  1.3391400000,-2.6000000000,36.0,0.0000000000,-4,
  -1.0422300000,-5.0000000000,27.0,0.0000000000,4,
  -0.7023630000,-3.5921200000,28.0,0.0000000000,-4,
  1.6798900000,-0.2000000000,22.0,0.0000000000,4,
  2.0208100000,1.4554700000,15.0,0.0000000000,-4,
  -0.3624070000,-0.7000000000,3.0,0.0000000000,4,
  -0.0223500000,0.6791620000,27.0,0.0000000000,-4,

  -22.7378860000,-7.0000000000,18.0,0.0000000000,4,
  -22.2260720000,-4.0000000000,23.0,0.0000000000,-4,
  -11.5139280000,5.2000000000,16.0,0.0000000000,4,
  -11.0021140000,7.7000000000,24.0,0.0000000000,-4,
  -21.7146850000,-0.4000000000,20.0,0.0000000000,4
  -21.2036880000,2.8000000000,16.0,0.0000000000,-4,
  -24.7902720000,-2.0000000000,34.0,0.0000000000,4,
  -24.2763210000,2.0000000000,20.0,0.0000000000,-4,
  -20.6930310000,6.5000000000,17.0,0.0000000000,4,
  -20.1826820000,9.5000000000,20.0,0.0000000000,-4,
  -23.7629680000,4.7000000000,20.0,0.0000000000,4,
  -23.2501720000,8.0000000000,22.0,0.0000000000,-4,
  -16.6153180000,-7.3000000000,24.0,0.0000000000,4,
  -16.1059380000,-4.2242330000,22.0,0.0000000000,-4,
  -19.6725940000,-9.0000000000,17.0,0.0000000000,4,
  -19.1627290000,-5.7000000000,20.0,0.0000000000,-4,
  -15.5964960000,-0.4000000000,8.0,0.0000000000,4,
  -15.0869540000,2.4000000000,10.0,0.0000000000,-4,
  -18.6530460000,-1.6000000000,12.0,0.0000000000,4
  -18.1435030000,1.2000000000,6.0,0.0000000000,-4,
  -14.5772710000,6.3000000000,15.0,0.0000000000,4,
  -14.0674050000,9.3000000000,20.0,0.0000000000,-4,
  -17.6340620000,5.2000000000,16.0,0.0000000000,4,
  -17.1246810000,8.2000000000,20.0,0.0000000000,-4,
  -10.4898290000,-7.0000000000,20.0,0.0000000000,4,
  -9.9770320000,-4.0000000000,13.0,0.0000000000,-4,
  -13.5573180000,-8.3000000000,28.0,0.0000000000,4,
  -13.0469680000,-5.2000000000,23.0,0.0000000000,-4,
  -9.4636790000,-0.1000000000,16.0,0.0000000000,4,
  -8.9497280000,2.3000000000,28.0,0.0000000000,-4,
  -12.5363130000,-1.5000000000,23.0,0.0000000000,4,
  -12.0253140000,1.1000000000,30.0,0.0000000000,-4
};

#define VELODYNE_BOTTOM_TO_TOP_BLOCK_DIST .0762f


typedef double T_corr_factors[5];
struct VelodyneFactors{
    float RCF,VCF;
    float RCF_SIN, RCF_COS;
    float VCF_SIN, VCF_COS;
    float HCF;
  };

class VelodyneReceiver{
public:
  VelodyneReceiver(car_timestamp_sync* vtsProvider, bool filter=true);	
  ~VelodyneReceiver();

  void AddScanSegmentCallback(VelodyneCallbackType cbk, void* param){
    scanSegmentCallback = cbk;
    scanSegmentCallbackParam = param;
  }

  
  void GetAndResetStats(double& timeSpentInCallbacks, int& numCallbacks, double& timeSpentInUserCallback){
    numCallbacks = numUDPCallbacks;
    timeSpentInCallbacks = timeSpentInUDPCallback;
    timeSpentInUserCallback = this->timeSpentInUserCallback;

    this->timeSpentInUDPCallback=0;
    //this->numUDPCallbacks=0;
    this->timeSpentInUserCallback=0;
  }

  static float GetBeamHeight(int laserNum, float rangeFromSensor, float sensorHeight){
    if(laserNum<0||laserNum>63) return -1000.f;
    
    if(laserNum<32){
      float v = (float)velodyne_bottom_block_laser_correction_factors[laserNum][1];  // sin(vcf)
      float t = 1.f/cosf((float)velodyne_top_block_laser_correction_factors[laserNum-32][3])*rangeFromSensor; // 1/cos(vcf)*range
      v = v*t;
      return v + sensorHeight;
    }else{
      float v = (float)velodyne_top_block_laser_correction_factors[laserNum-32][1];  // sin(vcf)
      float t = 1.f/cosf((float)velodyne_top_block_laser_correction_factors[laserNum-32][3])*rangeFromSensor; // 1/cos(vcf)*range
      v = v*t;
      return v + sensorHeight + VELODYNE_BOTTOM_TO_TOP_BLOCK_DIST;
    }


  }

protected:
  udp_connection* conn;
  void UDPCallback(udp_message& msg, udp_connection* conn, void* arg);
	
  VelodyneCallbackType scanSegmentCallback;
  void* scanSegmentCallbackParam;

  car_timestamp_sync* vtsProvider;

  VelodyneFactors factors[64];

  double timeSpentInUDPCallback;
  int numUDPCallbacks;
  double timeSpentInUserCallback;
	bool filterPoints;
};

#endif //VELODYNE_RECEIVER_H_SEPT_14_2007_SVL5
